#include "confSaveAndLoad.h"
#include "motor.h"
#include "crc_tools.h"
#include "string.h"
#include "my_flash.h"

bool conf_saveAndLoad_test(void)
{
  motor_config_t test_conf = {
    .abz_encode_lines = 1.1,
    .abz_zero_electric_Angle = 2.2,
    .c_kd = 3.3
    // ... 其他配置字段初始化 ...
  };

  printf("[TEST] Saving configuration...\n");
  if (!conf_save(&test_conf)) {
    printf("[ERROR] Save failed!\n");
    return false;
  }

  motor_config_t loaded_conf = {0};
  printf("[TEST] Loading configuration...\n");
  if (!conf_load(&loaded_conf)) {
    // printf("[ERROR] Load failed!\n");
    return false;
  }

  // 验证加载的配置是否与保存的一致
  if (memcmp(&test_conf, &loaded_conf, sizeof(motor_config_t))) {
    printf("[ERROR] Configuration mismatch!\n");
    return false;
  }

  printf("abz_encode_lines: %d ", loaded_conf.abz_encode_lines);
  printf("abz_zero_electric_Angle: %f ", loaded_conf.abz_zero_electric_Angle);
  printf("c_kd: %f\n", loaded_conf.c_kd);

  printf("[TEST] Save/Load test passed!\n");
  return true;
}

bool conf_save(motor_config_t* conf)
{
  SaveHeader saveheader = {0};
  saveheader.header = 0xAA;
  saveheader.payloadlen = sizeof(motor_config_t);

  uint8_t frame[512];

  size_t frame_len = assemble_frame(&saveheader, (uint8_t*)conf, sizeof(motor_config_t), frame, sizeof(frame));

  printf("frame_len: %d\n", frame_len);

  printf("Frame bytes: ");
  for (size_t i = 0; i < frame_len; i++) {
    printf("%02x ", frame[i]);
  }
  printf("\n");

  Flash_Write((uint32_t *)frame, frame_len);

  return true;
}

bool conf_load(motor_config_t* conf)
{
  // 1. 读帧头，取负载长度，计算帧长度
  SaveHeader saveheader = {0};
  size_t header_len = sizeof(SaveHeader);
  Flash_Read((uint32_t*)&saveheader, header_len);
  printf("payloadlen: %d\n", saveheader.payloadlen);
  size_t aligned_payload_len = (saveheader.payloadlen + 3) & ~3;
  size_t frame_len =  header_len + aligned_payload_len + sizeof(uint32_t);
  printf("frame_len: %d\n", frame_len);

    // 2. 读整个帧
  uint8_t frame[256];
  Flash_Read((uint32_t*)frame, frame_len);
  printf("read Frame bytes: ");
  for (size_t i = 0; i < frame_len; i++) {
    printf("%02x ", frame[i]);
  }
  printf("\n");

  // 3. CRC 校验
  uint32_t read_crc;
  memcpy(&read_crc, frame + header_len + aligned_payload_len, sizeof(uint32_t));
  uint32_t calc_crc = crc32(frame, header_len + aligned_payload_len);

  if (read_crc != calc_crc)
  {
    printf("read_crc = %u, calc_crc = %u\n", read_crc, calc_crc);
    return false;
  }

  // 4. 取出负载
  // uint8_t payload[saveheader.payloadlen];
  memcpy(conf, frame + header_len, saveheader.payloadlen);
  return true;
}

// 帧组装函数
size_t assemble_frame(
  SaveHeader *saveheader, 
  uint8_t *payload, 
  size_t payload_len,
  uint8_t *frame,
  size_t max_frame_len
)
{
  size_t aligned_payload_len = (payload_len + 3) & ~3;

  size_t header_len = sizeof(SaveHeader);

  size_t frame_len = header_len + aligned_payload_len + sizeof(uint32_t);

  if (frame_len > max_frame_len)
  {
    return 0; // 错误返回帧长度为0
  }
  
  // 复制帧头到输出帧
  memcpy(frame, saveheader, header_len);

  // 复制负载到输出帧
  memcpy(frame + header_len, payload, payload_len);
  memset(frame + header_len + payload_len, 0, aligned_payload_len - payload_len);

  // 计算 CRC 校验值
  uint32_t crc = crc32(frame, header_len + aligned_payload_len);

  // 添加CRC校验值到输出帧
  memcpy(frame + header_len + aligned_payload_len, &crc, sizeof(uint32_t));

  return frame_len;
}
